#include <stdlib.h>
#include <stdint.h>
#include <stdarg.h>
#include "ak7010.h"
#include "can.h"
#include "math.h"
#include "main.h"

#define PI 3.14159265358979f

/** RECIEVE **/
/******************************************************
*       
*
    位置位 0高1低 范围-32000~32000代表位置-3200°~3200°
    速度位 2高3低 范围-32000~32000代表-320000~320000rpm 电气转速
    电流位 4高5低 数值-6000~6000代表-60~60A
    温度位   6    范围-20~127代表驱动板温度-20℃~127℃
    报错位   7    0为无故障，1为电机过温度故障，2为过电流故障，3为过压故障，4为欠压故障，5为编码器故障，6为mos管过温度故障，7为电机堵转
*
*
*******************************************************/

MOTOR_FEEDBACK AK7010_Data = {0};
temp_ak ak70_temp = {0};
temp_view view = {0};

void AK7010_data(uint8_t rx_data[8]){
      
      AK7010_Data.position_d = (float)((rx_data[0] << 8 | rx_data[1]) * 0.1f);        //位置
      AK7010_Data.speed = (float)((rx_data[2] << 8 | rx_data[3]) * 10.0f);          //速度
      AK7010_Data.current = (float)((rx_data[4]<<8 | rx_data[5]) * 0.01f);          //电流
      AK7010_Data.temperature = rx_data[6];                                         //温度
      AK7010_Data.error = rx_data[7];                                               //报错
      
}


/*******************************************************************

*************************伺服模式***********************************

*****************************************************************/
/** SEND **/
void comm_can_transmit_eid(uint32_t id, const uint8_t *data, uint8_t len) {
    
     if (len > 8){
        len = 8;
     }
     
     CAN_TxHeaderTypeDef Tx;
     Tx.StdId = 0;//0x200
     
     Tx.IDE = CAN_ID_EXT;
//     Tx.IDE = CAN_ID_STD;
     Tx.ExtId = id;
     Tx.RTR = CAN_RTR_DATA;
     Tx.DLC = len;
     Tx.TransmitGlobalTime = DISABLE;
     
     uint32_t send_mail_box = 0;
     
     HAL_CAN_AddTxMessage(&hcan2, &Tx, data, &send_mail_box);
     
}

void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) {
  
    buffer[(*index)++] = number >> 24;
    buffer[(*index)++] = number >> 16;
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
    
}


void buffer_append_int16(uint8_t* buffer, int16_t number, int16_t *index) {
  
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
    
}


void AK7010_DutyMode(uint8_t controller_id, float duty) {
   
    int32_t send_index = 0;
    uint8_t buffer[4];
    buffer_append_int32(buffer, (int32_t)(duty * 100000.0), &send_index);
    comm_can_transmit_eid(controller_id |((uint32_t)CAN_PACKET_SET_DUTY << 8), buffer, send_index);
    
}


void AK7010_CurrentMode(uint8_t controller_id, float current) {
  
    int32_t send_index = 0;
    uint8_t buffer[4];
    buffer_append_int32(buffer, (int32_t)(current * 1000.0), &send_index);
    comm_can_transmit_eid(controller_id |((uint32_t)CAN_PACKET_SET_CURRENT << 8), buffer, send_index);
    
}

void AK7010_CurrentBreakMode(uint8_t controller_id, float current) {
  
    int32_t send_index = 0;
    uint8_t buffer[4];
    buffer_append_int32(buffer, (int32_t)(current * 1000.0), &send_index);
    comm_can_transmit_eid(controller_id |((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8), buffer, send_index);
}


void AK7010_RPMMode(uint8_t controller_id, float rpm)
{
  
    int32_t send_index = 0;
    uint8_t buffer[4];
    buffer_append_int32(buffer, (int32_t)rpm, &send_index);
    comm_can_transmit_eid((controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8)), buffer, send_index);
    
}



void AK7010_PosMode(uint8_t controller_id, float pos) {
  
    int32_t send_index = 0;
    uint8_t buffer[4];
    buffer_append_int32(buffer, (int32_t)(pos * 10000.0), &send_index);

    comm_can_transmit_eid(controller_id | ((uint32_t)CAN_PACKET_SET_POS << 8), buffer, send_index);
    
}



void AK7010_OriginMode(uint8_t controller_id, uint8_t set_origin_mode) {
  //0代表设置临时原点(断电消除)，1代表设置永久零点(仅限双编码器机型);

//    int32_t send_index = 0;
//    uint8_t buffer[4];
//    comm_can_transmit_eid(controller_id | ((uint32_t) CAN_PACKET_SET_ORIGIN_HERE << 8), buffer, send_index);
    int32_t send_index = 0;
    uint8_t buffer;
    buffer = set_origin_mode;
    comm_can_transmit_eid(controller_id |((uint32_t) CAN_PACKET_SET_ORIGIN_HERE << 8), &buffer, send_index);
}

void AK7010_Pos_SpdMode(uint8_t controller_id, float pos, int16_t spd, int16_t RPA) 
{
    int32_t send_index = 0;
    int16_t send_index1 = 0;
    uint8_t buffer[4];
    buffer_append_int32(buffer, (int32_t)(pos * 10000.0), &send_index);
    buffer_append_int16(buffer, spd, &send_index1);
    buffer_append_int16(buffer, RPA, &send_index1);
    comm_can_transmit_eid(controller_id | ((uint32_t)CAN_PACKET_SET_POS_SPD << 8), buffer, send_index);
}


/*******************************************************************

*************************运控模式***********************************

********************************************************************/

float P_MIN =-12.5f;
float P_MAX =12.5f;
float V_MIN =-50.0f;
float V_MAX =50.0f;
float T_MIN =-25.0f;
float T_MAX =25.0f;
float I_MAX = 60.f;
float Kp_MIN =0;
float Kp_MAX =500.0f;
float Kd_MIN =0;
float Kd_MAX =5.0f;
float Test_Pos=0.0f;

void Can_senddata_to_AK70(uint8_t *buff, int len){
  CAN_TxHeaderTypeDef txHeader;
  uint32_t mailbox = 0;
  
  if(len > 8)
    len = 8;
  txHeader.StdId = 0x1;
//  txHeader.ExtId = 0x1;
  txHeader.IDE = CAN_ID_STD;
  txHeader.RTR = CAN_RTR_DATA;
  txHeader.DLC = len;
  txHeader.TransmitGlobalTime = DISABLE;
  
  HAL_CAN_AddTxMessage(&hcan2, &txHeader, buff, &mailbox);
  
 /*
    进入电机控制模式 {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0XFC }
    退出电机控制模式 {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, 0XFD }
    设置电机当前位置为0点 {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0XFE }
    如果是在无状态下想要读取当前的状态，发送的命令是{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,0XFC }
*/
}

int float_to_uint(float x, float x_min, float x_max, unsigned int bits){
 /// Converts a float to an unsigned int, given range and number of bits ///
     float span = x_max- x_min;
     if(x < x_min) x = x_min;
     else if(x > x_max) x = x_max;
     return (int) ((x- x_min)*((float)((1<<bits)/span)));
}

float uint_to_float(int x_int, float x_min, float x_max, int bits){
 /// converts unsigned int to float, given range and number of bits ///
     float span = x_max- x_min;
     float offset = x_min;
     return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
 }


void Mit_pack_cmd(float p_des, float v_des, float t_ff, float kp, float kd){
 /// limit data to be within bounds ///
     
     uint8_t Buff[8] = {0};
     
     p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
     v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
     kp = fminf(fmaxf(Kp_MIN, kp), Kp_MAX);
     kd = fminf(fmaxf(Kd_MIN, kd), Kd_MAX);
     t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
     
     /// convert floats to unsigned ints ///
     int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
     int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
     int kp_int = float_to_uint(kp, Kp_MIN, Kp_MAX, 12);
     int kd_int = float_to_uint(kd, Kd_MIN, Kd_MAX, 12);
     int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
     /// pack ints into the can buffer ///

     Buff[0] = p_int>>8;//位置高 8
     Buff[1] = p_int&0xFF; //位置低 8
     
     Buff[2] = v_int>>4;//速度高 8 位
     Buff[3] = ((v_int&0xF)<<4)|(kp_int>>8); //速度低 4 位 KP 高 4 位
     Buff[4] = kp_int&0xFF; //KP 低 8 位
     
     Buff[5] = kd_int>>4;//Kd 高 8 位
     Buff[6] = ((kd_int&0xF)<<4)|(t_int>>8);//KP 低 4 位扭矩高 4 位
     Buff[7] = t_int&0xff;//扭矩低 8 位
     
     Can_senddata_to_AK70(Buff, 8);
     
 }

void Mit_unpack_reply(uint8_t rx_data[]){
 /// unpack ints from can buffer ///
//     int id = rx_data[0]; //驱动 ID 号
     int p_int = ( rx_data[1] << 8 ) | rx_data[2];
     int v_int = ( rx_data[3] << 4 ) | ( rx_data [4] >> 4 );
     int i_int = (( rx_data[4] & 0xF ) << 8 ) | rx_data[5];
     int T_int = rx_data[6] ;
     
     /// convert ints to floats ///
     AK7010_Data.id = rx_data[0];
     AK7010_Data.position_r = uint_to_float(p_int, P_MIN, P_MAX, 16);//电机位置数据 单位 rad
     AK7010_Data.position_d = uint_to_float(p_int, P_MIN, P_MAX, 16) * (180 / PI); //电机位置数据 单位 deg
     AK7010_Data.speed = uint_to_float(v_int, V_MIN, V_MAX, 12);//电机速度数据 单位 rad/s
     AK7010_Data.current = uint_to_float(i_int,-I_MAX, I_MAX, 12);//电机扭矩数据 单位 N.M
     AK7010_Data.temperature = T_int - 40;//电机温度 单位 °C
     AK7010_Data.error = rx_data[7];

 }

